#include "Quad4ThermalCalculator.h"
#include "SGFem/Integration/IntegrationFormula/Gauss2D2X2P.h"

#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Element/IElementService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"
#include "DBManager/IService/Input/Material/IThermalMaterialService.h"
#include "DBManager/IService/Input/Property/IPSHELLService.h"

#include "DataStructure/Input/Element/CQUAD4Data.h"
#include "DataStructure/Input/Property/PSHELLData.h"
#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/ModelInfoQuery.h"

using SG::Algebra::Point;
using SG::Algebra::Vector3D;
using SG::Algebra::Matrix;
using SG::Algebra::identity;
using SG::DataStructure::Common::Real;
using SG::DataStructure::Common::Id;
using SG::DataStructure::FEM::ThermalMaterial;
using namespace SG::DataStructure::FEM;
using namespace SG::DBManager;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::Element::Gauss2D2X2P;
using SG::FEMSolver::Element::Quad4ThermalCalculator;
std::size_t Quad4ThermalCalculator::s_nNode = 4;
std::size_t Quad4ThermalCalculator::s_nDim = 2;


void Quad4ThermalCalculator::Initialize (SG::DataStructure::FEM::ElementTypes type)
{
    m_type = type;
    const auto& integ = m_integForm.GetPoints ();
    m_shpFunc.Compute (integ);
}

/// @brief 使用求逆犯规法计算积分点Jacobian,并且组装应变矩阵`B`
void Quad4ThermalCalculator::ComputeStrainMatrix(std::size_t iGauss, const Matrix& coord, _OUT Matrix& B, _OUT Real& j) const
{
    const auto& DNDxi = m_shpFunc.GetShapeFunctionDerivative(iGauss);
    auto JT = DNDxi * coord;
    j = JT(0, 0) * JT(1, 1) - JT(0, 1) * JT(1, 0);
    auto JTinv = SG::Algebra::inverse2X2 (JT);
    auto DNDx = JTinv * DNDxi;

    B = Matrix(s_nDim, s_nNode);
    B = DNDx;
}

void Quad4ThermalCalculator::ComputeStiffness
(
    const Matrix& coord,    
    const ThermalMaterial& material,
    const Real& thickness,
    _OUT Matrix& ke
)
{
    const Real& kx = material.m_K;  //热传导系数
    const Real& ky = material.m_K;  //热传导系数
    Matrix D(s_nDim, s_nDim);
    D(0,0) = kx;
    D(1,1) = ky;

    ke = Matrix(s_nNode, s_nNode);
    
    Matrix B;
    Real j;
    const auto nGauss = m_integForm.GetPointSize();
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        ComputeStrainMatrix(iGauss, coord, B, j);
        const Real tmp = j * m_integForm.GetPoint(iGauss).m_weight;
        auto BT = B.Transpose ();
        ke  += BT * D * B * tmp;  //单元传热矩阵
    }

    ke = ke * thickness;
}

void Quad4ThermalCalculator::ComputeCapacityStiffness
    (
    const SG::Algebra::Matrix& vertices, 
    const SG::DataStructure::FEM::ThermalMaterial& material,
    const Real& thickness,
    _OUT SG::Algebra::Matrix& ke3
    )
{
    Real m_rho = material.m_RHO;
    Real m_c = material.m_CP;

    ke3 = Matrix(s_nNode, s_nNode);
    
    Matrix B;
    Real j;
    const auto nGauss = m_integForm.GetPointSize();     
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        ComputeStrainMatrix(iGauss, vertices, B, j);
        const Real tmp = j * m_integForm.GetPoint(iGauss).m_weight;
        auto& DN = m_shpFunc.GetShapeFunction(iGauss);
        auto DNT = DN.Transpose ();
        ke3  += DN * DNT * tmp;  
    }

    ke3 = ke3 * (m_rho * m_c * thickness);
}

void Quad4ThermalCalculator::ComputeHeatSourceLoad
    (
    const SG::Algebra::Matrix& coords,
    const std::vector<SG::DataStructure::Common::Real>& q,
    const Real& thickness,
    _OUT SG::Algebra::Matrix& p1
    )
{
    if (q.size() != 4) {
        // 丢出错误并终止程序
        throw std::runtime_error("Error: The Heat resource points of the Quad4 element should be 4.");
    }
    
    Matrix B;
    Real j;
    const auto nGauss = m_integForm.GetPointSize();     
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        ComputeStrainMatrix (iGauss, coords, B, j);
        const Real tmp = j * m_integForm.GetPoint(iGauss).m_weight;
        auto& DN = m_shpFunc.GetShapeFunction(iGauss);
        const Real qsum = q[0] * DN(0, 0) + q[1] * DN(1, 0) + q[2] * DN(2, 0) + q[3] * DN(3, 0);
        p1  += DN * (tmp * qsum);
    }
    p1 = p1 * thickness;
}


void Quad4ThermalCalculator::ComputeElementMatries (const SG::DBManager::DBServiceFactorySP dbServiceFactory)
{
    auto& assembly = AssemblyEigen::GetInstance ();
    
    static const DofSet DOFs {DOFDirections::DOF_Ux};

    SG::DataStructure::FEM::CQUAD4Data eleData;
    PSHELLData property;
    ThermalMaterial material;
    
    std::vector<std::size_t> nodeIndexes(s_nNode);
    std::vector<Id> dispCoordIds(s_nNode);
    std::vector<int> dofGlobalIndex (s_nNode);
    std::vector<Point> points(s_nNode);
    
    const auto& quad4Service = dbServiceFactory ->get<IElementService>();
    const auto& nodeService = dbServiceFactory -> get<INodeService>();
    const auto eleSize = quad4Service->GetElementSize (m_type);
    for (std::size_t index{0}; index < eleSize; ++index)
    {
        /** 从数据库获取对应的单元数据 */
        quad4Service->FindByIndex (index, _OUT eleData);

        // Node coordinate
        ModelInfoQuery::QueryNode (nodeService, eleData.m_g, s_nNode, _OUT points, _OUT nodeIndexes, _OUT dispCoordIds);
        Matrix coord(s_nNode,s_nDim);
        for (std::size_t iNode = 0; iNode < s_nNode; ++iNode)
        {
            coord(iNode, 0) = points[iNode].m_x;
            coord(iNode, 1) = points[iNode].m_y;
        }
        assembly.GetGlobalDOFId (nodeIndexes, DOFs, _OUT dofGlobalIndex);
         
        dbServiceFactory ->get<IPSHELLService>()->FindById (eleData.m_pId, _OUT property);
        dbServiceFactory ->get<IThermalMaterialService>()->FindById (property.m_mId1, _OUT material);
        
        Matrix ke1(s_nNode, s_nNode);
        ComputeStiffness(coord, material, property.m_thickness, _OUT ke1);
        assembly.AccumulateStiffness(dofGlobalIndex, ke1);
    }
}


// 后处理数据
void Quad4ThermalCalculator::DataRecovery (const SG::DBManager::DBServiceFactorySP dbServiceFactory, int subcaseId, SG::DataStructure::Common::Real time, const std::vector<SG::DataStructure::Common::Real>& globalDisplacement)
{
}

// 计算全局坐标系下单元应力刚度矩阵
void Quad4ThermalCalculator::ComputeStressStiffness (const SG::DBManager::DBServiceFactorySP dbServiceFactory, int staticSubId)
{
}
